03. Get the Starter Code
Python to C++
Note: We have recently added a project workspace to the classroom that you can use for coding this project, which you can find in a few pages from now. The workspace already has the code downloaded in it if you choose to use that option, but you should still read the below instructions.
First, click here to download the C++ starter code. [Note: This was recently refactored to include header files, as well as updated to remove an out-of-bounds issue with the
test_sense()function].Open the code in your favorite editor. You'll probably want to have the corresponding Python code around to consult as well.
Fill out the functions in
localizer.cppandhelpers.cpp
NOTE - when compiling your code, make sure you use C++11. You can do this from the command line with the following:
g++ -std=c++11 tests.cppAre you having trouble getting the code?
Students in China have not been able to download the project starter code. If you were able to download the starter code you can ignore everything below.
As a temporary solution, we are including the code as text below. You will need to copy and paste the text into files with the correct names. If you have any problems please let us know in the Student Hub channel and we will help you ASAP.
Directory Structure
You should create a new project folder/directory to put the project code into. Inside that you should also create a maps directory.
Then you should create empty files within those directories to match the following image:
File text (for .cpp and .txt files)
debugging_helpers.cpp
/**
debugging_helpers.cpp
Purpose: helper functions for debugging when working
with grids of floats and chars.
*/
#include <vector>
using namespace std;
/**
Displays a grid of beliefs. Does not return.
@param grid - a two dimensional grid (vector of
vectors of floats) which will usually
represent a robot's beliefs.
*/
void show_grid(vector < vector <float> > grid) {
int i, j;
float p;
vector<float> row;
for (i = 0; i < grid.size(); i++)
{
row = grid[i];
for (j=0; j< row.size(); j++)
{
p = row[j];
cout << p << ' ';
}
cout << endl;
}
}
/**
Displays a grid map of the world
*/
void show_grid(vector < vector <char> > map) {
int i, j;
char p;
vector<char> row;
for (i = 0; i < map.size(); i++)
{
row = map[i];
for (j=0; j< row.size(); j++)
{
p = row[j];
cout << p << ' ';
}
cout << endl;
}
}helpers.cpp
/**
helpers.cpp
Purpose: helper functions which are useful when
implementing a 2-dimensional histogram filter.
This file is incomplete! Your job is to make the
normalize and blur functions work. Feel free to
look at helper.py for working implementations
which are written in python.
*/
#include <vector>
#include <iostream>
#include <cmath>
#include <string>
#include <fstream>
// #include "debugging_helpers.cpp"
using namespace std;
/**
TODO - implement this function
Normalizes a grid of numbers.
@param grid - a two dimensional grid (vector of vectors of floats)
where each entry represents the unnormalized probability
associated with that grid cell.
@return - a new normalized two dimensional grid where the sum of
all probabilities is equal to one.
*/
vector< vector<float> > normalize(vector< vector <float> > grid) {
vector< vector<float> > newGrid;
// todo - your code here
return newGrid;
}
/**
TODO - implement this function.
Blurs (and normalizes) a grid of probabilities by spreading
probability from each cell over a 3x3 "window" of cells. This
function assumes a cyclic world where probability "spills
over" from the right edge to the left and bottom to top.
EXAMPLE - After blurring (with blurring=0.12) a localized
distribution like this:
0.00 0.00 0.00
0.00 1.00 0.00
0.00 0.00 0.00
would look like this:
0.01 0.02 0.01
0.02 0.88 0.02
0.01 0.02 0.01
@param grid - a two dimensional grid (vector of vectors of floats)
where each entry represents the unnormalized probability
associated with that grid cell.
@param blurring - a floating point number between 0.0 and 1.0
which represents how much probability from one cell
"spills over" to it's neighbors. If it's 0.0, then no
blurring occurs.
@return - a new normalized two dimensional grid where probability
has been blurred.
*/
vector < vector <float> > blur(vector < vector < float> > grid, float blurring) {
vector < vector <float> > newGrid;
// your code here
return normalize(newGrid);
}
/** -----------------------------------------------
#
#
# You do not need to modify any code below here.
#
#
# ------------------------------------------------- */
/**
Determines when two grids of floating point numbers
are "close enough" that they should be considered
equal. Useful for battling "floating point errors".
@param g1 - a grid of floats
@param g2 - a grid of floats
@return - A boolean (True or False) indicating whether
these grids are (True) or are not (False) equal.
*/
bool close_enough(vector < vector <float> > g1, vector < vector <float> > g2) {
int i, j;
float v1, v2;
if (g1.size() != g2.size()) {
return false;
}
if (g1[0].size() != g2[0].size()) {
return false;
}
for (i=0; i<g1.size(); i++) {
for (j=0; j<g1[0].size(); j++) {
v1 = g1[i][j];
v2 = g2[i][j];
if (abs(v2-v1) > 0.0001 ) {
return false;
}
}
}
return true;
}
bool close_enough(float v1, float v2) {
if (abs(v2-v1) > 0.0001 ) {
return false;
}
return true;
}
/**
Helper function for reading in map data
@param s - a string representing one line of map data.
@return - A row of chars, each of which represents the
color of a cell in a grid world.
*/
vector <char> read_line(string s) {
vector <char> row;
size_t pos = 0;
string token;
string delimiter = " ";
char cell;
while ((pos = s.find(delimiter)) != std::string::npos) {
token = s.substr(0, pos);
s.erase(0, pos + delimiter.length());
cell = token.at(0);
row.push_back(cell);
}
return row;
}
/**
Helper function for reading in map data
@param file_name - The filename where the map is stored.
@return - A grid of chars representing a map.
*/
vector < vector <char> > read_map(string file_name) {
ifstream infile(file_name);
vector < vector <char> > map;
if (infile.is_open()) {
char color;
vector <char> row;
string line;
while (std::getline(infile, line)) {
row = read_line(line);
map.push_back(row);
}
}
return map;
}
/**
Creates a grid of zeros
For example:
zeros(2, 3) would return
0.0 0.0 0.0
0.0 0.0 0.0
@param height - the height of the desired grid
@param width - the width of the desired grid.
@return a grid of zeros (floats)
*/
vector < vector <float> > zeros(int height, int width) {
int i, j;
vector < vector <float> > newGrid;
vector <float> newRow;
for (i=0; i<height; i++) {
newRow.clear();
for (j=0; j<width; j++) {
newRow.push_back(0.0);
}
newGrid.push_back(newRow);
}
return newGrid;
}
// int main() {
// vector < vector < char > > map = read_map("maps/m1.txt");
// show_grid(map);
// return 0;
// }localizer.cpp
/**
localizer.cpp
Purpose: implements a 2-dimensional histogram filter
for a robot living on a colored cyclical grid by
correctly implementing the "initialize_beliefs",
"sense", and "move" functions.
This file is incomplete! Your job is to make these
functions work. Feel free to look at localizer.py
for working implementations which are written in python.
*/
#include "helpers.cpp"
#include <stdlib.h>
#include "debugging_helpers.cpp"
using namespace std;
/**
TODO - implement this function
Initializes a grid of beliefs to a uniform distribution.
@param grid - a two dimensional grid map (vector of vectors
of chars) representing the robot's world. For example:
g g g
g r g
g g g
would be a 3x3 world where every cell is green except
for the center, which is red.
@return - a normalized two dimensional grid of floats. For
a 2x2 grid, for example, this would be:
0.25 0.25
0.25 0.25
*/
vector< vector <float> > initialize_beliefs(vector< vector <char> > grid) {
vector< vector <float> > newGrid;
// your code here
return newGrid;
}
/**
TODO - implement this function
Implements robot sensing by updating beliefs based on the
color of a sensor measurement
@param color - the color the robot has sensed at its location
@param grid - the current map of the world, stored as a grid
(vector of vectors of chars) where each char represents a
color. For example:
g g g
g r g
g g g
@param beliefs - a two dimensional grid of floats representing
the robot's beliefs for each cell before sensing. For
example, a robot which has almost certainly localized
itself in a 2D world might have the following beliefs:
0.01 0.98
0.00 0.01
@param p_hit - the RELATIVE probability that any "sense" is
correct. The ratio of p_hit / p_miss indicates how many
times MORE likely it is to have a correct "sense" than
an incorrect one.
@param p_miss - the RELATIVE probability that any "sense" is
incorrect. The ratio of p_hit / p_miss indicates how many
times MORE likely it is to have a correct "sense" than
an incorrect one.
@return - a normalized two dimensional grid of floats
representing the updated beliefs for the robot.
*/
vector< vector <float> > sense(char color,
vector< vector <char> > grid,
vector< vector <float> > beliefs,
float p_hit,
float p_miss)
{
vector< vector <float> > newGrid;
// your code here
return normalize(newGrid);
}
/**
TODO - implement this function
Implements robot motion by updating beliefs based on the
intended dx and dy of the robot.
For example, if a localized robot with the following beliefs
0.00 0.00 0.00
0.00 1.00 0.00
0.00 0.00 0.00
and dx and dy are both 1 and blurring is 0 (noiseless motion),
than after calling this function the returned beliefs would be
0.00 0.00 0.00
0.00 0.00 0.00
0.00 0.00 1.00
@param dy - the intended change in y position of the robot
@param dx - the intended change in x position of the robot
@param beliefs - a two dimensional grid of floats representing
the robot's beliefs for each cell before sensing. For
example, a robot which has almost certainly localized
itself in a 2D world might have the following beliefs:
0.01 0.98
0.00 0.01
@param blurring - A number representing how noisy robot motion
is. If blurring = 0.0 then motion is noiseless.
@return - a normalized two dimensional grid of floats
representing the updated beliefs for the robot.
*/
vector< vector <float> > move(int dy, int dx,
vector < vector <float> > beliefs,
float blurring)
{
vector < vector <float> > newGrid;
// your code here
return blur(newGrid, blurring);
}simulate.cpp
/**
simulate.cpp
Purpose: implements a Simulation class which
simulates a robot living in a 2D world. Relies
on localization code from localizer.py
*/
#include "localizer.cpp"
#include <algorithm>
// #include "helpers.cpp"
class Simulation {
private:
vector <char> get_colors() {
vector <char> all_colors;
char color;
int i,j;
for (i=0; i<height; i++) {
for (j=0; j<width; j++) {
color = grid[i][j];
if(std::find(all_colors.begin(), all_colors.end(), color) != all_colors.end()) {
/* v contains x */
} else {
all_colors.push_back(color);
cout << "adding color " << color << endl;
/* v does not contain x */
}
}
}
colors = all_colors;
num_colors = colors.size();
return colors;
}
public:
vector < vector <char> > grid;
vector < vector <float> > beliefs;
float blur, p_hit, p_miss, incorrect_sense_prob;
int height, width, num_colors;
std::vector<int> true_pose;
std::vector<int> prev_pose;
vector <char> colors;
Simulation(vector < vector<char> >, float, float, vector <int>);
};
/**
Constructor for the Simulation class.
*/
Simulation::Simulation(vector < vector <char> > map,
float blurring,
float hit_prob,
std::vector<int> start_pos
)
{
grid = map;
blur = blurring;
p_hit = hit_prob;
p_miss = 1.0;
beliefs = initialize_beliefs(map);
incorrect_sense_prob = p_miss / (p_hit + p_miss);
true_pose = start_pos;
prev_pose = true_pose;
}
/**
You can test your code by running this function.
Do that by first compiling this file and then
running the output.
*/
// int main() {
// vector < vector <char> > map;
// vector <char> mapRow;
// int i, j, randInt;
// char color;
// std::vector<int> pose(2);
// for (i = 0; i < 4; i++)
// {
// mapRow.clear();
// for (j=0; j< 4; j++)
// {
// randInt = rand() % 2;
// if (randInt == 0 ) {
// color = 'r';
// }
// else {
// color = 'g';
// }
// mapRow.push_back(color);
// }
// map.push_back(mapRow);
// }
// cout << "map is\n";
// Simulation simulation (map, 0.1, 0.9, pose);
// // simulation = Simulation(map, 0.1, 0.9, pose);
// cout << "initialization success!\n";
// show_grid(map);
// cout << "x, y = (" << simulation.true_pose[0] << ", " << simulation.true_pose[1] << ")" << endl;
// return 0;
// }tests.cpp
#include <iostream>
#include "simulate.cpp"
bool test_normalize() {
vector < vector <float> > unnormalized, normalized, result;
unnormalized = zeros(2, 2);
normalized = zeros(2,2);
int i,j;
for (i=0; i<2; i++) {
for(j=0; j<2; j++) {
unnormalized[i][j] = 1.0;
normalized[i][j] = 0.25;
}
}
result = normalize(unnormalized);
bool correct;
correct = close_enough(normalized, result);
if (correct) {
cout << "! - normalize function worked correctly!\n";
}
else {
cout << "X - normalize function did not work correctly.\n";
cout << "For the following input:\n\n";
show_grid(unnormalized);
cout << "\nYour code returned the following:\n\n";
show_grid(result);
cout << "\nWhen it should have returned the following:\n";
show_grid(normalized);
}
return correct;
}
bool test_blur() {
vector < vector <float> > in, correct, out;
in = zeros(3, 3);
correct = zeros(3,3);
in[1][1] = 1.0;
float corner = 0.01;
float side = 0.02;
float center = 0.88;
correct[0][0] = corner;
correct[0][1] = side;
correct[0][2] = corner;
correct[1][0] = side;
correct[1][1] = center;
correct[1][2] = side;
correct[2][0] = corner;
correct[2][1] = side;
correct[2][2] = corner;
out = blur(in, 0.12);
bool right;
right = close_enough(correct, out);
if (right) {
cout << "! - blur function worked correctly!\n";
}
else {
cout << "X - blur function did not work correctly.\n";
cout << "For the following input:\n\n";
show_grid(in);
cout << "\nYour code returned the following:\n\n";
show_grid(out);
cout << "\nWhen it should have returned the following:\n";
show_grid(correct);
}
return right;
}
bool test_helpers() {
bool correct = true;
bool question_correct;
question_correct = test_normalize();
if (!question_correct) {
correct = false;
}
cout << endl;
question_correct = test_blur();
if (!question_correct) {
correct = false;
}
return correct;
}
bool test_initialize() {
vector < vector <char> > map;
map = read_map("maps/m1.txt");
int h = map.size();
if (h < 1) {
cout << "failed to load map. Make sure there is a maps/ directory in the same directory as this file!\n";
return false;
}
vector < vector <float> > beliefs, correct;
beliefs = initialize_beliefs(map);
int w, A;
float belief;
w = map[0].size();
A = h * w;
belief = 1.0 / A;
int i, j;
vector <float> row;
for (i=0; i<map.size(); i++) {
row.clear();
for (j=0; j<map[0].size(); j++) {
row.push_back(belief);
}
correct.push_back(row);
}
bool right = close_enough(correct, beliefs);
if (right) {
cout << "! - initialize_beliefs function worked correctly!\n";
}
else {
cout << "X - initialize_beliefs function did not work correctly.\n";
cout << "For the following input:\n\n";
show_grid(map);
cout << "\nYour code returned the following:\n\n";
show_grid(beliefs);
cout << "\nWhen it should have returned the following:\n";
show_grid(correct);
}
return right;
}
bool test_move() {
vector < vector <float> > in, out, correct;
in = zeros(3,3);
in[2][2] = 1.0;
int dx, dy;
dx = 1;
dy = 1;
float blurring = 0.0;
correct = zeros(3,3);
correct[0][0] = 1.0;
out = move(dy, dx, in, blurring);
bool right = close_enough(correct, out);
if (right) {
cout << "! - move function worked correctly with zero blurring\n";
}
else {
cout << "X - move function did not work correctly.\n";
cout << "When dx=1, dy=1, blurring=0.0 and with\nthe following beliefs:\n\n";
show_grid(in);
cout << "\nYour code returned the following:\n\n";
show_grid(out);
cout << "\nWhen it should have returned the following:\n";
show_grid(correct);
}
return right;
}
bool test_sense() {
vector < vector <float> > in, out, correct;
in = zeros(4,2);
in[2][1] = 1.0;
int i,j;
for (i=0; i<in.size(); i++)
{
for (j=0; j<in[0].size(); j++) {
in[i][j] = 1.0/8.0;
}
}
char color = 'r';
vector < vector <char> > map;
map = read_map("maps/half_red.txt");
float p_hit, p_miss;
p_hit = 2.0;
p_miss = 1.0;
out = sense(color, map, in, p_hit, p_miss);
float total = 0.0;
for (i=0; i<out.size(); i++)
{
for (j=0; j<out[0].size(); j++) {
total += out[i][j];
}
}
bool right = true;
if ( (total < 0.99) || (total > 1.01) ) {
right = false;
}
if ( (out.size() != in.size()) || out[0].size() != in[0].size()) {
right = false;
cout << "X - sense function not working correctly.\n";
cout << "Your function returned a grid with incorrect dimensions.\n";
return right;
}
float r_prob, g_prob, r_exp, g_exp;
r_prob = out[0][0];
g_prob = out[0][1];
r_exp = 1.0 / 6.0;
g_exp = 1.0 / 12.0;
if (close_enough(r_prob, r_exp) && close_enough(g_prob, g_exp)) {
cout << "! - sense function worked correctly\n";
return false;
}
else {
cout << "X - sense function did not work correctly.\n";
cout << "When p_hit=2.0, p_miss=1.0 and with\nthe following beliefs:\n\n";
show_grid(in);
cout << "\nYour code returned the following:\n\n";
show_grid(out);
cout << "\nbut this is incorrect.\n";
}
return right;
}
bool test_localizer() {
bool correct = true;
bool question_correct;
question_correct = test_initialize();
if (!question_correct) {
correct = false;
}
if (!correct) {
// map could not be loaded
return false;
}
cout << endl;
question_correct = test_move();
if (!question_correct) {
correct = false;
}
cout << endl;
question_correct = test_sense();
if (!question_correct) {
correct = false;
}
return correct;
}
// bool test_simulation() {
// // todo
// }
int main() {
cout << endl;
test_helpers();
test_localizer();
cout << endl;
return 0;
}maps/half_red.txt
r g
g r
r r
g g maps/m1.txt
r r r
r g r
r r r maps/m2.txt
r g
r r
g g